zero copy shm fastdds

#zero-copy   #shm   #fastdds  

---

Table of Content

ros2_shm_demo/
├── CMakeLists.txt
├── LICENSE
├── msg
│   └── ShmTopic.msg
├── package.xml
└── src
    ├── listener.cpp
    └── talker.cpp

msg

ShmTopic.msg

# A generic char array allows passing arbitrary data.
#
char[256] data
uint8 size
uint64 counter

uint8 MAX_SIZE=255

src

talker

talker.cpp

#include <chrono>
#include <cstring>
#include <memory>
#include <string>
#include <utility>

#include "rclcpp/rclcpp.hpp"

#include "ros2_shm_demo/msg/shm_topic.hpp"

using namespace std::chrono_literals;

class Talker : public rclcpp::Node {
private:
  using Topic = ros2_shm_demo::msg::ShmTopic;

public:
  explicit Talker(const rclcpp::NodeOptions &options)
      : Node("shm_demo_talker", options) {

    auto publishMessage = [this]() -> void {
      auto loanedMsg = m_publisher->borrow_loaned_message();

      populateLoanedMessage(loanedMsg);

      m_publisher->publish(std::move(loanedMsg));
      m_count++;
    };

    rclcpp::QoS qos(rclcpp::KeepLast(10));
    m_publisher = this->create_publisher<Topic>("chatter", qos);
    m_timer = this->create_wall_timer(1s, publishMessage);
  }

private:
  uint64_t m_count = 1;
  rclcpp::Publisher<Topic>::SharedPtr m_publisher;
  rclcpp::TimerBase::SharedPtr m_timer;

  void populateLoanedMessage(rclcpp::LoanedMessage<Topic> &loanedMsg) {
    Topic &msg = loanedMsg.get();

    // Create the data.
    // In general this will not be constant.
    // Ideally we would create it in place but the ROS API does not allow
    // that. Therefore we need to copy it to the loaned message.

    std::string payload{"Hello World"};

    // We can track a quasi dynamic (bounded) size like this to avoid
    // copying more data than needed.
    msg.size = (uint8_t)std::min(payload.size(), (size_t)Topic::MAX_SIZE);
    msg.counter = m_count;

    // Note that msg.data is a std::array generated by the IDL compiler
    std::memcpy(msg.data.data(), payload.data(), msg.size);

    RCLCPP_INFO(this->get_logger(), "Publishing %s %lu", payload.c_str(),
                msg.counter);
  }
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  rclcpp::spin(std::make_shared<Talker>(options));
  rclcpp::shutdown();

  return 0;
}

listener

listener.cpp

#include <cstring>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "ros2_shm_demo/msg/shm_topic.hpp"

class Listener : public rclcpp::Node {
private:
  using Topic = ros2_shm_demo::msg::ShmTopic;

public:
  explicit Listener(const rclcpp::NodeOptions &options)
      : Node("shm_demo_listener", options) {

    // subscription callback to process arriving data
    auto callback = [this](const Topic::SharedPtr msg) -> void {
      // Read the message and perform operations accordingly.
      // Here we copy the data and display it.

      std::memcpy(m_lastData, msg->data.data(), msg->size);
      m_lastData[Topic::MAX_SIZE] =
          '\0'; // in case there was no zero termination

      RCLCPP_INFO(this->get_logger(), "Received %s %lu", m_lastData,
                  msg->counter);
    };

    rclcpp::QoS qos(rclcpp::KeepLast(10));
    m_subscription = create_subscription<Topic>("chatter", qos, callback);
  }

private:
  rclcpp::Subscription<Topic>::SharedPtr m_subscription;

  char m_lastData[256];
};

int main(int argc, char *argv[]) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions options;
  rclcpp::spin(std::make_shared<Listener>(options));
  rclcpp::shutdown();

  return 0;
}

CMakeLists

cmake_minimum_required(VERSION 3.8)
project(ros2_shm_demo)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(rclcpp REQUIRED)

include_directories(include)
#msg
set(msg_files
  "msg/ShmTopic.msg"
)

rosidl_generate_interfaces(${PROJECT_NAME}
  ${msg_files}
)
ament_export_dependencies(rosidl_default_runtime)

# talker
add_executable(talker
   src/talker.cpp
)
ament_target_dependencies(talker
   "rclcpp"
)
rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
target_link_libraries(talker
  ${cpp_typesupport_target}
)
install(TARGETS talker DESTINATION lib/${PROJECT_NAME})

# listener
add_executable(listener
  src/listener.cpp
)

ament_target_dependencies(listener
  "rclcpp"
)
rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
target_link_libraries(listener
  ${cpp_typesupport_target}
)
install(TARGETS listener DESTINATION lib/${PROJECT_NAME})

endif()

ament_package()